package es.uji.icc.robot.fanuc;

import java.io.ByteArrayInputStream;
import java.io.IOException;
import java.text.DecimalFormat;
import java.text.DecimalFormatSymbols;
import java.text.ParseException;

import javax.xml.bind.annotation.XmlRootElement;
import javax.xml.parsers.DocumentBuilderFactory;
import javax.xml.parsers.ParserConfigurationException;

import org.w3c.dom.Element;
import org.xml.sax.SAXException;


@XmlRootElement(name="RobotPosition",namespace="")
public class RobotPosition {
	
	private int joint1;
	private int joint2;
	private int joint3;
	private int joint4;
	private int joint5;
	private int joint6;
	private long x;
	private long y;
	private long z;
	private long w;
	private long p;
	private long r;
	
	
	
	public RobotPosition(){
		
	}
	
	public int getJoint1() {
		return joint1;
	}
	public void setJoint1(int joint1) {
		this.joint1 = joint1;
	}
	public int getJoint2() {
		return joint2;
	}
	public void setJoint2(int joint2) {
		this.joint2 = joint2;
	}
	public int getJoint3() {
		return joint3;
	}
	public void setJoint3(int joint3) {
		this.joint3 = joint3;
	}
	public int getJoint4() {
		return joint4;
	}
	public void setJoint4(int joint4) {
		this.joint4 = joint4;
	}
	public int getJoint5() {
		return joint5;
	}
	public void setJoint5(int joint5) {
		this.joint5 = joint5;
	}
	public int getJoint6() {
		return joint6;
	}
	public void setJoint6(int joint6) {
		this.joint6 = joint6;
	}
	public long getX() {
		return x;
	}
	public void setX(long x) {
		this.x = x;
	}
	public long getY() {
		return y;
	}
	public void setY(long y) {
		this.y = y;
	}
	public long getZ() {
		return z;
	}
	public void setZ(long z) {
		this.z = z;
	}
	public long getW() {
		return w;
	}
	public void setW(long w) {
		this.w = w;
	}
	public long getP() {
		return p;
	}
	public void setP(long p) {
		this.p = p;
	}
	public long getR() {
		return r;
	}
	public void setR(long r) {
		this.r = r;
	}
	

	
	

	private long stringToLong(String stringValue) throws RobotResponseException {
		Number numberValue;
		Long longValue;
		DecimalFormatSymbols tailoredFormat = new DecimalFormatSymbols();
		tailoredFormat.setDecimalSeparator(',');
		String mask = "#########,#############";
		DecimalFormat df = new DecimalFormat(mask, tailoredFormat);
		try {
			numberValue = df.parse(stringValue);
		} catch (ParseException e) {
			throw new RobotResponseException(e.getMessage());
		}
		longValue = numberValue.longValue();
		return longValue;
	}
	

	public RobotPosition(String xml) throws RobotResponseException {	    
		
		Element node;		
		PointXYZ locationxyz = new PointXYZ();
		PointXYZ orientationxyz = new PointXYZ();		
		Joints joint = new Joints();

		try {						
			
			node = DocumentBuilderFactory.newInstance().newDocumentBuilder()
					.parse(new ByteArrayInputStream(xml.getBytes())).getDocumentElement();
			locationxyz.setX(stringToLong(node.getElementsByTagName("x").item(0).getFirstChild()
					.getNodeValue()));
			locationxyz.setY(stringToLong(node.getElementsByTagName("y").item(0).getFirstChild()
					.getNodeValue()));
			locationxyz.setZ(stringToLong(node.getElementsByTagName("z").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setX(stringToLong(node.getElementsByTagName("w").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setY(stringToLong(node.getElementsByTagName("p").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setZ(stringToLong(node.getElementsByTagName("r").item(0).getFirstChild()
					.getNodeValue()));
            this.x = locationxyz.getX();
            this.y = locationxyz.getY();
            this.z = locationxyz.getZ();
            this.w = orientationxyz.getX();
            this.p = orientationxyz.getY();
            this.r = orientationxyz.getZ();            

		} catch (SAXException e) {
			throw new RobotResponseException(e.getMessage());
		} catch (IOException e) {
			throw new RobotResponseException(e.getMessage());
		} catch (ParserConfigurationException e) {
			throw new RobotResponseException(e.getMessage());
		}

        try {						
			
			node = DocumentBuilderFactory.newInstance().newDocumentBuilder()
					.parse(new ByteArrayInputStream(xml.getBytes())).getDocumentElement();
			joint.setJoint1(Integer.parseInt(node.getElementsByTagName("joint1").item(0).getFirstChild()
					.getNodeValue()));
			joint.setJoint2(Integer.parseInt(node.getElementsByTagName("joint2").item(0).getFirstChild()
					.getNodeValue()));
			joint.setJoint3(Integer.parseInt(node.getElementsByTagName("joint3").item(0).getFirstChild()
					.getNodeValue()));
			joint.setJoint4(Integer.parseInt(node.getElementsByTagName("joint4").item(0).getFirstChild()
					.getNodeValue()));
			joint.setJoint5(Integer.parseInt(node.getElementsByTagName("joint5").item(0).getFirstChild()
					.getNodeValue()));
			joint.setJoint6(Integer.parseInt(node.getElementsByTagName("joint6").item(0).getFirstChild()
					.getNodeValue()));
            this.joint1 = joint.getJoint1();
            this.joint2 = joint.getJoint2();
            this.joint3 = joint.getJoint3();
            this.joint4 = joint.getJoint4();
            this.joint5 = joint.getJoint5();
            this.joint6 = joint.getJoint6();
            

		} catch (SAXException e) {
			throw new RobotResponseException(e.getMessage());
		} catch (IOException e) {
			throw new RobotResponseException(e.getMessage());
		} catch (ParserConfigurationException e) {
			throw new RobotResponseException(e.getMessage());
		}

	
}
	
}
